TYPE_ACC = 1
TYPE_GYR = 2
TYPE_MAG = 3
#
class StepEstimator:
    MIN_STEP_INTERVAL = 400000000
    MAX_STEP_INTERVAL = 1300000000

    THRESHOLD_POS = 0.2
    THRESHOLD_NEG = -0.2
    THRESHOLD_POS_PEAK = 1.1
    THRESHOLD_NEG_PEAK = -1.1

    ALPHA = 0.5

    _stepCount = 0
    _state = 0

    _lastTime = -1

    _gravity = None

    _stepStartTime = -1
    _stepEndTime = -1
    _stepTime = 0

    _lastStepStartTime = -1

    _stepState2Time = 0
    _stepState6Time = 0
    _stepHalfTime = 0

    _vVel = 0
    _vDis = 0

    _lastVDis = 0

    _lastData = None
    _stepAcc = [0., 0.]
    _lastStepAcc = None
    _lastTwoStepAcc = None

    def inputPosturedData(self, type, data, timestamp):
        if type != TYPE_ACC:
            return

        if self._lastTime < 0:
            self._lastTime = timestamp
            return

        vAccel = data[2] + self._gravity

        vVelNew = self._vVel + vAccel * (timestamp - self._lastTime) / 1e9
        self._vDis = self._vDis + (self._vVel + vVelNew) / 2 * (timestamp - self._lastTime) / 1e9
        self._vVel = vVelNew

        if self._state == 0 or self._state == 8:
            self._vVel = 0
            self._vDis = 0

        if self._lastData is not None:
            vAccel = self.ALPHA * vAccel + (1 - self.ALPHA) * (self._lastData[2] + self._gravity)

        nextState = self.getNextState(vAccel)

        if nextState == 1 and nextState != self._state:
            self._stepStartTime = timestamp

        if nextState == 2 and nextState != self._state:
            self._stepState2Time = timestamp

        if nextState == 6 and nextState != self._state:
            self._stepState6Time = timestamp
            self._stepHalfTime = self._stepState6Time - self._stepState2Time

        if nextState == 8 and nextState != self._state:
            self._stepEndTime = timestamp
            self._stepTime = self._stepEndTime - self._stepStartTime

            p = self._stepHalfTime / self._stepTime

            if (self._stepTime >= self.MIN_STEP_INTERVAL) and (p > 0.4) and (p < 0.6):
                self._stepCount = self._stepCount + 1

                self._lastStepStartTime = self._stepStartTime

                timeDiff = self.stepTime / 1e6

                self._stepAcc[0] = self._stepAcc[0] / timeDiff
                self._stepAcc[1] = self._stepAcc[1] / timeDiff

                if self._lastStepAcc is not None:
                    self._lastTwoStepAcc = [self._lastStepAcc[0] + self._stepAcc[0],
                                            self._lastStepAcc[1] + self._stepAcc[1]]
                else:
                    self._lastTwoStepAcc = None

                self._lastStepAcc = self._stepAcc
                self._stepAcc = [0., 0.]
                self._lastVDis = self._vDis
            else:
                self.reset()

            nextState = 0

        if timestamp - self._stepStartTime >= self.MAX_STEP_INTERVAL:
            nextState = 0
            self.reset()

        if nextState != 0:
            timeDiff = (timestamp - self._lastTime) / 1e6
            if (nextState >= 2) and (nextState <= 5):
                self._stepAcc[0] = self._stepAcc[0] - timeDiff * data[0]
                self._stepAcc[1] = self._stepAcc[1] - timeDiff * data[1]
            else:
                self._stepAcc[0] = self._stepAcc[0] + timeDiff * data[0]
                self._stepAcc[1] = self._stepAcc[1] + timeDiff * data[1]

        self._state = nextState
        self._lastTime = timestamp
        self._lastData = data

    def reset(self):
        self._stepStartTime = -1
        self._stepEndTime = -1
        self._lastStepAcc = None
        self._lastTwoStepAcc = None
        self._stepAcc = [0., 0.]

    def setGravity(self, value):
        self._gravity = value

    def getStepCount(self):
        return self._stepCount + self._state / 8.0

    def clearStepCount(self):
        self._stepCount = 0

    def getStepDirection(self):
        return self._lastStepAcc

    def getDirection(self):
        return self._lastTwoStepAcc

    def getLastStepStart(self):
        return self._lastStepStartTime

    def getLastStepTime(self):
        return self._stepTime

    def getLastStepHalfTime(self):
        return self._stepHalfTime

    def getLastVDisplacement(self):
        return self._lastVDis

    def getEstimatedStrideLength(self, height):
        if self._stepTime == 0:
            return 0
        scale = ((1e9 / self._stepTime) - 1.57) / 0.23 * 0.08 + 0.37
        if scale < 0.2:
            scale = 0.2
        if scale > 0.42:
            scale = 0.42
        return scale * height


    _STATE_TABLE = [[0, 0, 0, 1, 1],
                    [1, 1, 1, 1, 2],
                    [3, 3, 3, 3, 2],
                    [4, 4, 4, 3, 3],
                    [5, 5, 4, 3, 3],
                    [6, 5, 4, 4, 4],
                    [6, 7, 7, 7, 7],
                    [7, 7, 8, 8, 8],
                    [0, 0, 0, 1, 1]]

    def getNextState(self, vAccel):
        index = 0
        if vAccel < self.THRESHOLD_NEG_PEAK:
            index = 4
            return self._STATE_TABLE[self._state][index]
        if vAccel < self.THRESHOLD_NEG:
            index = 3
            return self._STATE_TABLE[self._state][index]
        if vAccel <= self.THRESHOLD_POS:
            index = 2
            return self._STATE_TABLE[self._state][index]
        if vAccel <= self.THRESHOLD_POS_PEAK:
            index = 1
            return self._STATE_TABLE[self._state][index]
        return self._STATE_TABLE[self._state][index]







